package com.crunch.math;

import java.util.Arrays;

/**
 * A class for quanternions.
 */
public final class Quaternionf {
	float[] components; // r,i,j,k

	/**
	 * Constructs a quaternion with a real component of 0 and imaginary components (0,0,1).
	 */
	public Quaternionf() {
		components = new float[] { 0.0f, 0.0f, 0.0f, 1.0f };
	}

	/**
	 * Sets this quaternion to the identity quaternion.
	 */
	public void setIdentity() {
		components[0] = 0.0f;
		components[1] = 0.0f;
		components[2] = 0.0f;
		components[3] = 1.0f;
	}

	/**
	 * Returns whether this quaternion is the identity quaternion.
	 */
	public boolean isIdentity() {
		return components[0] == 0.0f && components[1] == 0.0f && components[2] == 0.0f && components[3] == 1.0f;
	}

	/**
	 * Constructs a quaternion by copying the one provided.
	 */
	public Quaternionf(Quaternionf q) {
		components = Arrays.copyOf(q.components, q.components.length);
	}

	/**
	 * Sets the quaternion by copying the one provided.
	 */
	public void set(Quaternionf q) {
		System.arraycopy(q.components, 0, components, 0, components.length);
	}

	/**
	 * Constructs a quaternion with the provided components.
	 */
	public Quaternionf(float r, float i, float j, float k) {
		components = new float[] { r, i, j, k };
	}

	/**
	 * Sets the quaternion to the provided components.
	 */
	public void set(float r, float i, float j, float k) {
		components[0] = r;
		components[1] = i;
		components[2] = j;
		components[3] = k;
	}

	/**
	 * Returns the <i>i</i>th component.
	 */
	public float get(int i) {
		return components[i];
	}

	/**
	 * Sets the <i>i</i>th component.
	 */
	public void set(int i, float s) {
		components[i] = s;
	}

	/**
	 * Returns the r-component.
	 */
	public float r() {
		return components[0];
	}

	/**
	 * Returns the i-component.
	 */
	public float i() {
		return components[1];
	}

	/**
	 * Returns the j-component.
	 */
	public float j() {
		return components[2];
	}

	/**
	 * Returns the k-component.
	 */
	public float k() {
		return components[3];
	}

	/**
	 * Sets the r-component.
	 */
	public void r(float s) {
		components[0] = s;
	}

	/**
	 * Sets the i-component.
	 */
	public void i(float s) {
		components[1] = s;
	}

	/**
	 * Sets the j-component.
	 */
	public void j(float s) {
		components[2] = s;
	}

	/**
	 * Sets the k-component.
	 */
	public void k(float s) {
		components[3] = s;
	}

	/**
	 * Constructs a quaternion from the axis/angle provided.
	 */
	public Quaternionf(AxisAngle aa) {
		float half = aa.getAngle() * 0.5f;
		Vector3f axis = aa.getAxis().multiply((float) Math.sin(half));
		components = new float[] { (float) Math.cos(half), axis.x(), axis.y(), axis.z() };
	}

	/**
	 * Sets the quaternion from the axis/angle provided.
	 */
	public void set(AxisAngle aa) {
		float half = aa.getAngle() * 0.5f;
		Vector3f axis = aa.getAxis().multiply((float) Math.sin(half));
		components[0] = (float) Math.cos(half);
		components[1] = axis.x();
		components[2] = axis.y();
		components[3] = axis.z();
	}

	/**
	 * Returns an axis/angle corresponding to this quaternion.
	 */
	public AxisAngle getAxisAngle() {
		float scale = (float) Math.sqrt(i()*i() + j()*j() + k()*k());
		if (scale == 0.0f) {
			return new AxisAngle(new Vector3f(0.0f, 0.0f, 1.0f), 0.0f);
		} else {
			float invScale = 1.0f / scale;
			return new AxisAngle(new Vector3f(i()*invScale, j()*invScale, k()*invScale), (float) Math.acos(r()) * 2.0f);
		}
	}

	/**
	 * Returns the sum of this quaternion and {@code rhs}.
	 */
	public Quaternionf add(Quaternionf rhs) {
		return new Quaternionf(this).addAssign(rhs);
	}

	/**
	 * Adds {@code rhs} to this quaternion.
	 */
	public Quaternionf addAssign(Quaternionf rhs) {
		for (int i = 0; i < components.length; ++i) {
			components[i] += rhs.components[i];
		}
		return this;
	}

	/**
	 * Returns the difference between this quaternion and {@code rhs}.
	 */
	public Quaternionf subtract(Quaternionf rhs) {
		return new Quaternionf(this).subtractAssign(rhs);
	}

	/**
	 * Subtracts {@code rhs} from this quaternion.
	 */
	public Quaternionf subtractAssign(Quaternionf rhs) {
		for (int i = 0; i < components.length; ++i) {
			components[i] -= rhs.components[i];
		}
		return this;
	}

	/**
	 * Returns this quaternion negated.
	 */
	public Quaternionf negate() {
		return new Quaternionf(this).negateAssign();
	}

	/**
	 * Negates this quaternion.
	 */
	public Quaternionf negateAssign() {
		for (int i = 0; i < components.length; ++i) {
			components[i] = -components[i];
		}
		return this;
	}

	/**
	 * Returns the product of this quaternion and {@code rhs}.
	 */
	public Quaternionf multiply(float rhs) {
		return new Quaternionf(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this quaternion by {@code rhs}.
	 */
	public Quaternionf multiplyAssign(float rhs) {
		for (int i = 0; i < components.length; ++i) {
			components[i] *= rhs;
		}
		return this;
	}

	/**
	 * Returns the quotient of this quaternion and {@code rhs}.
	 */
	public Quaternionf divide(float rhs) {
		return new Quaternionf(this).divideAssign(rhs);
	}

	/**
	 * Divides this quaternion by {@code rhs}.
	 */
	public Quaternionf divideAssign(float rhs) {
		for (int i = 0; i < components.length; ++i) {
			components[i] /= rhs;
		}
		return this;
	}

	/**
	 * Returns the product this quaternion and {@code rhs}.
	 */
	public Quaternionf multiply(Quaternionf rhs) {
		return new Quaternionf(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this quaternion by {@code rhs}.
	 */
	public Quaternionf multiplyAssign(Quaternionf rhs) {
		float r = r()*rhs.r() - i()*rhs.i() - j()*rhs.j() - k()*rhs.k();
		float i = r()*rhs.i() + i()*rhs.r() + j()*rhs.k() - k()*rhs.j();
		float j = r()*rhs.j() - i()*rhs.k() + j()*rhs.r() + k()*rhs.i();
		float k = r()*rhs.k() + i()*rhs.j() - j()*rhs.i() + k()*rhs.r();
		components[0] = r;
		components[1] = i;
		components[2] = j;
		components[3] = k;
		return this;
	}

	/**
	 * Returns the product of this quaternion and and {@code rhs}.
	 */
	public Vector3f multiply(Vector3f rhs) {
		Quaternionf vq = new Quaternionf(0.0f, rhs.x(), rhs.y(), rhs.z());
		// this * vq * conjugate
		Quaternionf ret = multiply(vq).multiply(conjugate());
		return new Vector3f(ret.i(), ret.j(), ret.k());
	}

	/**
	 * Returns whether this vector is equal to the object {@code o}.
	 */
	@Override
	public boolean equals(Object o) {
		if (o == this) {
			return true;
		} else if (o == null || !(o instanceof Quaternionf)) {
			return false;
		} else {
			return equals((Quaternionf) o);
		}
	}

	/**
	 * Returns a hash code for this object.
	 */
	@Override
	public int hashCode() {
		int bits = 0;
		for (int i = 0; i < components.length; ++i) {
			bits ^= Float.floatToIntBits(components[i]);
		}
		return bits;
	}

	/**
	 * Returns whether this vector is equal to the quaternion {@code rhs}.
	 */
	public boolean equals(Quaternionf rhs) {
		return Arrays.equals(components, rhs.components);
	}

	/**
	 * Returns this quaternion normalized.
	 */
	public Quaternionf normalize() {
		return new Quaternionf(this).normalizeAssign();
	}

	/**
	 * Normalizes this quaternion.
	 */
	public Quaternionf normalizeAssign() {
		float mag2 = magnitudeSquared();
		if (mag2 == 0.0f) {
			Arrays.fill(components, 0.0f);
		} else {
			float invMag = 1.0f / (float) Math.sqrt(mag2);
			multiply(invMag);
		}
		return this;
	}

	/**
	 * Returns the conjugate of this quaternion.
	 */
	public Quaternionf conjugate() {
		return new Quaternionf(this).conjugateAssign();
	}

	/**
	 * Conjugates this quaternion.
	 */
	public Quaternionf conjugateAssign() {
		components[1] = -components[1];
		components[2] = -components[2];
		components[3] = -components[3];
		return this;
	}

	/**
	 * Returns the magnitude of this vector.
	 */
	public float magnitude() {
		return (float) Math.sqrt(magnitudeSquared());
	}

	/**
	 * Returns the squared magnitude of this vector.
	 */
	public float magnitudeSquared() {
		return r()*r() + i()*i() + j()*j() + k()*k();
	}

	/**
	 * Returns the raw underlying mutable array.
	 */
	public float[] getArray() {
		return components;
	}
}
